﻿// <copyright file="Quaternion.cs" company="Axelerate Solutions S.A.">
// Copyright (c) Axelerate Solutions S.A.  All rights reserved.
// </copyright>
namespace Axelerate.Silverlight3D.Media.Media3D
{
    using System;
    using System.Net;
    using System.Windows;
    using System.Windows.Controls;
    using System.Windows.Documents;
    using System.Windows.Ink;
    using System.Windows.Input;
    using System.Windows.Media;
    using System.Windows.Media.Animation;
    using System.Windows.Shapes;
    using System.ComponentModel;

    /// <summary>
    /// File name: Quaternion.cs
    /// Company: Axelerate Solutions S.A.
    /// Created By: Daniel Rojas
    /// Created Date: 10/10/2008 11:47:00 h.
    /// Description: Structure that represents a rotation in three dimensions.
    ///   <Modifications>
    ///   </Modifications>
    /// </summary>
    [TypeConverterAttribute(typeof(QuaternionConverter))]
    public struct Quaternion : IFormattable
    {
        #region Fields

        /// <summary>
        /// Store the X component of the Quaternion.
        /// </summary>
        private double mX;
        
        /// <summary>
        /// Store the Y component of the Quaternion.
        /// </summary>
        private double mY;

        /// <summary>
        /// Store the Z component of the Quaternion.
        /// </summary>
        private double mZ;

        /// <summary>
        /// Store the W component of the Quaternion.
        /// </summary>
        private double mW;

        /// <summary>
        /// Store the identy quaternion.
        /// </summary>
        private static Quaternion s_identity;

        /// <summary>
        /// Used to know if this instance of the quaternion is the wellknow quaternion identity
        /// </summary>
        private bool _isNotDistinguishedIdentity;

        #endregion

        #region Properties

        /// <summary>
        /// Gets the X component of the Quaternion.
        /// </summary>
        public double X
        {
            get
            {
                return mX;
            }
            set
            {
                mX = value;
            }
        }

        /// <summary>
        /// Gets the Y component of the Quaternion.
        /// </summary>
        public double Y
        {
            get
            {
                return mY;
            }
            set
            {
                mY = value;
            }
        }

        /// <summary>
        /// Gets the Z component of the Quaternion.
        /// </summary>
        public double Z
        {
            get
            {
                return mZ;
            }
            set
            {
                mZ = value;
            }
        }

        /// <summary>
        /// Gets the W component of the Quaternion.
        /// </summary>
        public double W
        {
            get
            {
                return mW;
            }
            set
            {
                mW = value;
            }
        }

        /// <summary>
        /// Get a value indicating wheter this instance of the Quaternion structure is the Identity instance.
        /// </summary>
        private bool IsDistinguishedIdentity
        {
            get
            {
                return !this._isNotDistinguishedIdentity;
            }
        }

        /// <summary>
        /// Gets the Quaternion's angle, in degrees.
        /// </summary>
        /// <value>Double that represents the Quaternion's angle, in degrees.</value>
        public double Angle
        {
            get
            {

                if (this.IsDistinguishedIdentity)
                {
                    return 0;
                }

                if ((this.X == 0) && (this.Y == 0) && (this.Z == 0) && (this.W == 0))
                {
                    return 0;
                }

                Quaternion q = this;
                q.Normalize();
                return (180 * 2 * System.Math.Acos(q.W)) / System.Math.PI;
            }
        }

        /// <summary>
        /// Gets the identity Quaternion
        /// </summary>
        public static Quaternion Identity
        {
            get
            {
                return Quaternion.s_identity;
            }
        }

        /// <summary>
        /// Gets a value that indicates whether the specified Quaternion is an identity Quaternion.
        /// </summary>
        public bool IsIdentity
        {
            get
            {
                return (this.IsDistinguishedIdentity  ||
                       ((this.X == 0) &&
                        (this.Y == 0) &&
                        (this.Z == 0) &&
                        (this.W == 1.0)));
            }
        }
        
        /// <summary>
        /// Gets a value that indicates whether the Quaternion is normalized.
        /// </summary>
        public bool IsNormalized
        {
            get
            {
                if (this.IsDistinguishedIdentity)
                {
                    return true;
                }

                return System.Math.Round(this.Lenght, 4) == 1.0;
            }
        }
        #endregion

        #region Constructors
        
        /// <summary>
        /// static constructor. Incialize the identity property
        /// </summary>
        static Quaternion()
        {
            Quaternion.s_identity = new Quaternion(0, 0, 0, 1);
            Quaternion.s_identity._isNotDistinguishedIdentity = false;
        }

        /// <summary>
        /// Creates a new Quaternion.
        /// </summary>
        /// <param name="x">Value of the new Quaternion's X coordinate.</param>
        /// <param name="y">Value of the new Quaternion's Y coordinate.</param>
        /// <param name="z">Value of the new Quaternion's Z coordinate.</param>
        /// <param name="w">Value of the new Quaternion's W coordinate.</param>
        public Quaternion(double x, double y, double z, double w)
        {
            this.mX = x;
            this.mY = y;
            this.mZ = z;
            this.mW = w;
            this._isNotDistinguishedIdentity = true;
        }

        /// <summary>
        /// Creates a new Quaternion.
        /// </summary>
        /// <param name="axisOfRotation">Vector3D that represents the axis of rotation.</param>
        /// <param name="angleInDegrees">Angle to rotate around the specified axis, in degrees.</param>
        public Quaternion(Vector3D axisOfRotation, double angleInDegrees)
        {
            this._isNotDistinguishedIdentity = true;
            double halfAngleInRadians = (angleInDegrees * 0.5 * System.Math.PI) / 180;
            
            axisOfRotation.Normalize();
            this.mX = axisOfRotation.X * System.Math.Sin(halfAngleInRadians);
            this.mY = axisOfRotation.Y * System.Math.Sin(halfAngleInRadians);
            this.mZ = axisOfRotation.Z * System.Math.Sin(halfAngleInRadians);
            this.mW = System.Math.Cos(halfAngleInRadians);

        }
        #endregion

        #region Operators

        /// <summary>
        /// Adds the specified Quaternions.
        /// </summary>
        /// <param name="left">First quaternion to add.</param>
        /// <param name="right">Second quaternion to add.</param>
        /// <returns>Quaternion that is the sum of the two specified Quaternions.</returns>
        public static Quaternion operator +(Quaternion left, Quaternion right)
        {
            return Quaternion.Add(left, right);
        }

        /// <summary>
        /// Multiplies the specified quaternion by another.
        /// </summary>
        /// <param name="left">First quaternion.</param>
        /// <param name="right">Second quaternion.</param>
        /// <returns>Quaternion that is the product of multiplication.</returns>
        public static Quaternion operator *(Quaternion left, Quaternion right)
        {
            return Quaternion.Multiply(left, right);
        }

        /// <summary>
        /// Subtracts a specified quaternion from another.
        /// </summary>
        /// <param name="left">Quaternion from which to subtract.</param>
        /// <param name="right">Quaternion to subtract from the first quaternion.</param>
        /// <returns>Quaternion that is the result of subtraction.</returns>
        public static Quaternion operator -(Quaternion left, Quaternion right)
        {
            return Quaternion.Subtract(left, right);
        }

        /// <summary>
        /// Compares two Quaternion instances for exact equality.
        /// </summary>
        /// <param name="quaternion1">First Quaternion to compare.</param>
        /// <param name="quaternion2">Second Quaternion to compare.</param>
        /// <returns>True if the two Quaternion instances are exactly equal, false otherwise.</returns>
        /// <remarks>Note that values of type Double might acquire error when operated upon; an exact comparison between two logically-equal values might fail. Furthermore, when using this equality operator, Double.NaN is not equal to itself.</remarks>
        public static bool operator ==(Quaternion quaternion1, Quaternion quaternion2)
        {
            return Quaternion.Equals(quaternion1, quaternion2); 
        }

        /// <summary>
        /// Compares two Quaternion instances for exact inequality.
        /// </summary>
        /// <param name="quaternion1">First Quaternion to compare.</param>
        /// <param name="quaternion2">Second Quaternion to compare.</param>
        /// <returns>True if the two Quaternion instances are not exactly equal, false otherwise.</returns>
        /// <remarks>Note that values of type Double might acquire error when operated upon; an exact comparison between two logically-equal values might fail. Furthermore, when using this equality operator, Double.NaN is not equal to itself.</remarks>
        public static bool operator !=(Quaternion quaternion1, Quaternion quaternion2)
        {
            return !Quaternion.Equals(quaternion1, quaternion2);
        }
        #endregion

        #region Methods

        /// <summary>
        /// Returns a normalized quaternion.
        /// </summary>
        /// <remarks>Throws InvalidOperationException if the quaternion is zero and cannot be normalized.</remarks>
        public void Normalize()
        {
            if (this.IsDistinguishedIdentity)
            {
                return;
            }

            double lenght = this.Lenght;
            this.X = this.X / lenght;
            this.Y = this.Y / lenght;
            this.Z = this.Z / lenght;
            this.W = this.W / lenght;
        }

        /// <summary>
        /// Internal method to caculate the "Lenght" of the quaternion (like a 4D Vector), to use this on the normalize function. 
        /// </summary>
        private double Lenght
        {
            get
            {
                return System.Math.Sqrt(this.X * this.X +
                                             this.Y * this.Y +
                                             this.Z * this.Z +
                                             this.W * this.W);
            }
        }

        /// <summary>
        /// Multiplies the specified Quaternions.
        /// </summary>
        /// <param name="left">First quaternion to multiply.</param>
        /// <param name="right">Second quaternion to multiply.</param>
        /// <returns>Quaternion that is the result of multiplication.</returns>
        public static Quaternion Multiply(Quaternion left, Quaternion right)
        {
            if (left.IsDistinguishedIdentity)
            {
                return right;
            }
            if (right.IsDistinguishedIdentity)
            {
                return left;
            }

            // q1q2 = w1w2 + w1q2 + w2q1 + q1 x q2 - q1.q2
            double xComponent = left.W * right.X + right.W * left.X + (left.Y * right.Z - left.Z * right.Y);
            double yComponent = left.W * right.Y + right.W * left.Y + (left.Z * right.X - left.X * right.Z);
            double zComponent = left.W * right.Z + right.W * left.Z + (left.X * right.Y - left.Y * right.X);
            double wComponent = left.W * right.W - (left.X * right.X + left.Y * right.Y + left.Z * right.Z);
            return new Quaternion(xComponent, yComponent, zComponent, wComponent);
        }

        /// <summary>
        /// Adds the specified quaternions.
        /// </summary>
        /// <param name="left">First quaternion to add.</param>
        /// <param name="right">Second quaternion to add.</param>
        /// <returns>Quaternion that is the result of addition.</returns>
        public static Quaternion Add(Quaternion left, Quaternion right)
        {
            if (left.IsDistinguishedIdentity)
            {
                if (right.IsDistinguishedIdentity)
                {
                    return new Quaternion(0, 0, 0, 2);
                }
                right.W = right.W + 1;
                return right;
            }
            if (right.IsDistinguishedIdentity)
            {
                left.W = left.W + 1;
                return left;
            }

            return new Quaternion(left.X + right.X,
                                  left.Y + right.Y,
                                  left.Z + right.Z,
                                  left.W + right.W);
        }

        /// <summary>
        /// Subtracts a Quaternion from another.
        /// </summary>
        /// <param name="left">Quaternion from which to subtract.</param>
        /// <param name="right">Quaternion to subtract from the first quaternion.</param>
        /// <returns>Quaternion to subtract from the first quaternion.</returns>
        public static Quaternion Subtract(Quaternion left, Quaternion right)
        {
            if (left.IsDistinguishedIdentity)
            {
                if (right.IsDistinguishedIdentity)
                {
                    return new Quaternion(0, 0, 0, 0);
                }
                return new Quaternion(-right.X, -right.Y, -right.Z, 1 - right.W);
            }
            if (right.IsDistinguishedIdentity)
            {
                return new Quaternion(left.X, left.Y, left.Z, left.W - 1);
            }

            return new Quaternion(left.X - right.X,
                                  left.Y - right.Y,
                                  left.Z - right.Z,
                                  left.W - right.W);
        }

        /// <summary>
        /// Compares two Quaternion instances for equality.
        /// </summary>
        /// <param name="quaternion1">First Quaternion to compare.</param>
        /// <param name="quaternion2">Second Quaternion to compare.</param>
        /// <returns>True if the two Quaternion instances are exactly equal, false otherwise.</returns>
        public static bool Equals(Quaternion quaternion1, Quaternion quaternion2)
        {
            return quaternion1.mW == quaternion2.mW && quaternion1.mX == quaternion2.mX &&
                   quaternion1.mY == quaternion2.mY && quaternion1.mZ == quaternion2.mZ; 
        }

        /// <summary>
        /// Compares two Quaternion instances for equality.
        /// </summary>
        /// <param name="value">Quaternion with which to compare.</param>
        /// <returns>True if the two Quaternion instances are exactly equal, false otherwise.</returns>
        public bool Equals(Quaternion value)
        {
            return Quaternion.Equals(this, value);
        }

        /// <summary>
        /// Compares two Quaternion instances for equality.
        /// </summary>
        /// <param name="o">Quaternion with which to compare.</param>
        /// <returns>Quaternion with which to compare.</returns>
        public override bool Equals(object o)
        {
            if (o is Quaternion)
            {
                return this.Equals((Quaternion)o);
            }
            else
            {
                return false;
            }
        }

        /// <summary>
        /// Replaces a quaternion with its conjugate.
        /// </summary>
        public void Conjugate()
        {
            if (!this.IsDistinguishedIdentity)
            {
                this.X *= -1;
                this.Y *= -1;
                this.Z *= -1;
            }
        }

        /// <summary>
        /// Replaces the specified quaternion with its inverse
        /// </summary>
        public void Invert()
        {
            double squaredLenght = (this.X * this.X + this.Y * this.Y + this.Z * this.Z + this.W * this.W);

            Quaternion q = this;
            q.Conjugate();
            this.X = q.X / squaredLenght;
            this.Y = q.Y / squaredLenght;
            this.Z = q.Z / squaredLenght;
            this.W = q.W / squaredLenght;
        }

        /// <summary>
        /// Creates a string representation of the object.
        /// </summary>
        /// <param name="provider">Culture-specific formatting information.</param>
        /// <returns>String representation of the object.</returns>
        public string ToString(IFormatProvider provider)
        {
            return string.Format(provider, "{0},{1},{2},{3}", this.mX, this.mY, this.mZ, this.mW); 
        }

        /// <summary>
        /// Creates a string representation of the object.
        /// </summary>
        /// <returns>String representation of the object.</returns>
        public override string ToString()
        {
            return string.Format("{0},{1},{2},{3}", this.mX, this.mY, this.mZ, this.mW);
        }

        /// <summary>
        /// Converts a string representation of a Quaternion into the equivalent Quaternion structure.
        /// </summary>
        /// <param name="source">Type: System.String "X,Y,Z,W"</param>
        /// <returns>Quaternion represented by the string.</returns>
        public static Quaternion Parse(string source)
        {
            if (source != "")
            {
                string[] parts = source.Split(new char[] { ',' }, StringSplitOptions.RemoveEmptyEntries);
                if (parts.Length == 4)
                {
                    return new Quaternion(double.Parse(parts[0]), double.Parse(parts[1]), double.Parse(parts[2]), double.Parse(parts[3]));
                }
            }

            throw new FormatException();
        }

        /// <summary>
        /// Quaternion that represents the orientation resulting from the interpolation.
        /// </summary>
        /// <param name="from">Quaternion that represents the starting orientation.</param>
        /// <param name="to">Quaternion that represents the starting orientation.</param>
        /// <param name="t">Quaternion that represents the starting orientation.</param>
        /// <returns>Quaternion that represents the orientation resulting from the interpolation.</returns>
        public static Quaternion Slerp(Quaternion from, Quaternion to, double t)
        {
            return Slerp(from, to, t, true);
        }

        /// <summary>
        /// Interpolates between orientations, represented as Quaternion structures, using spherical linear interpolation.
        /// </summary>
        /// <param name="from">Quaternion that represents the starting orientation.</param>
        /// <param name="to">Quaternion that represents the starting orientation.</param>
        /// <param name="t">Quaternion that represents the starting orientation.</param>
        /// <param name="useShortestPath">Boolean that indicates whether to compute quaternions that constitute the shortest possible arc on a four-dimensional unit sphere.</param>
        /// <returns>Quaternion that represents the orientation resulting from the interpolation.</returns>
        public static Quaternion Slerp(Quaternion from, Quaternion to, double t, bool useShortestPath)
        {
            // Slerp(p,q,t) = (sin((1-t)angle) * p + sin(t*angle) * q) / sin(angle)
            // Interpolate along a sphere between the two quaternions
            double fromLenght = from.Lenght;
            double toLenght = to.Lenght;
            from.Normalize();
            to.Normalize();
            double fromDotTo = from.X * to.X + from.Y * to.Y + from.Z * to.Z + from.W * to.W;

            if (useShortestPath)
            {
                if (fromDotTo < 0)
                {
                    // Need to negate one of the quaternions
                    to = new Quaternion(-to.X, -to.Y, -to.Z, -to.W);
                    fromDotTo *= -1;
                }
            }
            if (fromDotTo < -1)
            {
                fromDotTo = -1;
            }
            else if (fromDotTo > 1)
            {
                fromDotTo = 1;
            }

            double angleInRadians = System.Math.Acos(fromDotTo);
            double sinAngle = System.Math.Sin(angleInRadians);

            // Be careful about division by zero
            if (Math.Round(sinAngle, 4) == 0)
            {
                // Just lerp instead of slerp
                return new Quaternion(from.X + (to.X - from.X) * t,
                                      from.Y + (to.Y - from.Y) * t,
                                      from.Z + (to.Z - from.Z) * t,
                                      from.W + (to.W - from.W) * t);
            }
            else
            {

                double sinFrom = System.Math.Sin((1.0 - t) * angleInRadians) / sinAngle;
                double sinTo = System.Math.Sin(t * angleInRadians) / sinAngle;

                // Scale from FromNorm to toNorm depending on t.  When t==0 then
                // scale == fromNorm, when t==1 scale == toNorm
                double scale = fromLenght * System.Math.Pow((toLenght / fromLenght), t);
                return new Quaternion(scale * (from.X * sinFrom + to.X * sinTo),
                                      scale * (from.Y * sinFrom + to.Y * sinTo),
                                      scale * (from.Z * sinFrom + to.Z * sinTo),
                                      scale * (from.W * sinFrom + to.W * sinTo));
            }
        }

        /// <summary>
        /// Get the hashcode for this instance
        /// </summary>
        /// <returns>The integer that represent the hashcode for this object</returns>
        public override int GetHashCode()
        {
            return this.X.GetHashCode() | this.Y.GetHashCode() | this.Z.GetHashCode() | this.W.GetHashCode(); 
        }
        #endregion

        #region IFormattable Members

        public string ToString(string format, IFormatProvider formatProvider)
        {
            return this.ToString(formatProvider);
        }

        #endregion
    }
}
